#include <avr/interrupt.h>
#include <avr/io.h>
#include "CoreC.h"


//#define floorADC1 1 //    PF1
//#define floorADC2 4 //    PB4
//#define floorADC3 7 //    PD7
//#define floorADC4 6 //    PD6
//#define floorADC5 4 //    PF4
//#define floorLEDs 7 //    PB7
//
// ADC1,ADC11,ADC10,ADC9,ADC4
uint8_t FloorChannelsList[5] = {1, 11, 10, 9, 4};
uint8_t WallChannelsList[6] = {13, 7, 6, 1, 4, 5};

uint8_t ADCMuxCount=0;
uint8_t *DataPointer;

int8_t linePos = 0;
int8_t lastLinePos = 0;
 
void LineSensorsSetup()
{

}

void startFloorSensorMode()
{
  //input
  DDRB &= ~(1<<floorADC2);
  DDRD &= ~(1<<floorADC3|1<<floorADC4);
  DDRF &= ~(1<<floorADC1|1<<floorADC5);
    
  //ensure pullups are off
  PORTB &= ~(1<<floorADC2);
  PORTD &= ~(1<<floorADC3|1<<floorADC4);
  PORTF &= ~(1<<floorADC1|1<<floorADC5);

  //output
  //Turn on emitters
  DDRB |= 1<<floorLEDsInner;
  PORTB |= 1<<floorLEDsInner;
  DDRC |= 1<<floorLEDsOuter;
  PORTC |= 1<<floorLEDsOuter;

}



void AdmuxSetup( uint8_t AdcSignal )
{
      //setup ADC
  ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // Set ADC prescaler to 128 - 125KHz sample rate @ 16MHz
  ADMUX |= (1 << REFS0); // Set ADC reference to AVCC
  ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
  //ADCSRA |= (1 << ADSC);  // Set ADC to single conversion Mode
  ADCSRA |= (1 << ADEN);  // Enable ADC
  ADCSRA |= (1 << ADIE);  // Enable ADC Interrupt
  /* set up the mux to read the given signal */
  if ( AdcSignal >= 0x08 )
  {
    /* Set Mux5 */
    ADCSRB |= _BV( MUX5);
    AdcSignal &= ~0x08;
  }
  else
  {
    /* Clear Mux5 */
    ADCSRB &= ~_BV( MUX5);
  }
  ADMUX &= ~(0x1F);
  ADMUX |= (AdcSignal & 0x1F);
}

void stopFloorSensorMode()
{
  PORTB &= ~(1<<floorLEDsInner);
  PORTC &= ~(1<<floorLEDsOuter);

  ADCSRA &= ~(1 << ADEN);  // disable ADC
  ADCSRA &= ~(1 << ADIE);  // disable ADC Interrupt
}
void readLineSensors()
{
  AdmuxSetup(FloorChannelsList[ADCMuxCount]);
  ADCSRA |= (1 << ADSC);  // Start A2D Conversions  
}

ISR(ADC_vect)
{
    switch (mode.robotFunction)
    {
    case ROBOTFUNCTION_DRAGRACE:
    case ROBOTFUNCTION_LINEFOLLOWER:
      DataPointer = (uint8_t *)&lineSensorData;
      *(DataPointer+ADCMuxCount) = ADCH;
      ADCMuxCount++;
      if (ADCMuxCount == 5){  ADCMuxCount = 0;  }
     AdmuxSetup(FloorChannelsList[ADCMuxCount]);
     //ADCSRA |= (1 << ADSC);  // Start again 
    break;
    case ROBOTFUNCTION_WALLFOLLOWER:
      DataPointer = (uint8_t *)&wallSensorData;
      *(DataPointer+ADCMuxCount) = ADCH;
      ADCMuxCount++;
      if (ADCMuxCount == 6){  ADCMuxCount = 0;  }
      AdmuxSetup(WallChannelsList[ADCMuxCount]);
      
      //ADCSRA |= (1 << ADSC);  // Start again
    break;
    }
 }
  
void getLinePos()
{  
  //int8_t M = lineSensorData.sensor3 / 2;
  int8_t L = (int8_t)lineSensorData.sensor2 / 2;
  int8_t R = (int8_t)lineSensorData.sensor4 / 2;
  linePos = (0-L)+(0+R);//+lastLinePos; 
  if (linePos>127)
  {
    linePos=127;
  }
    if (linePos < -127)
    {
      linePos=-127;
    }
  //lastLinePos = linePos;
}
void trimToLine()
{
  if (linePos > 20)
  {
    trim =trimlevel/*(linePos)*/;
  }
  else if (linePos < -20)
  {
    trim = -trimlevel;//(linePos);
  }
  else
  {
    trim = 0;
    
  }
}


